from utime import sleep
from machine import Pin

# 设定上中下三个电机的引脚
yju = [18, 19, 20, 21]  # P-,P+,D-,D+
yjm = [10, 11, 12, 13]
yjd = [6, 7, 8, 9]

# 初始化电机
mu1 = Pin(yju[0], Pin.OUT)
mu2 = Pin(yju[1], Pin.OUT)
mu3 = Pin(yju[2], Pin.OUT)
mu4 = Pin(yju[3], Pin.OUT)

mm1 = Pin(yjm[0], Pin.OUT)
mm2 = Pin(yjm[1], Pin.OUT)
mm3 = Pin(yjm[2], Pin.OUT)
mm4 = Pin(yjm[3], Pin.OUT)

md1 = Pin(yjd[0], Pin.OUT)
md2 = Pin(yjd[1], Pin.OUT)
md3 = Pin(yjd[2], Pin.OUT)
md4 = Pin(yjd[3], Pin.OUT)


# 初始化电机主程序
def set_move(pos, w1, w2, w3, w4):  # pos确定电机位置（上中下），w1-w4是电机驱动器电平
    if pos == 'u':
        mu1.value(w1)
        mu2.value(w2)
        mu3.value(w3)
        mu4.value(w4)
    elif pos == 'm':
        mm1.value(w1)
        mm2.value(w2)
        mm3.value(w3)
        mm4.value(w4)
    elif pos == 'd':
        md1.value(w1)
        md2.value(w2)
        md3.value(w3)
        md4.value(w4)

# 设定前向运动函数
def move_forward(pos, delay, steps):
    for i in range(steps):
        set_move(pos=pos, w1=1, w2=0, w3=0, w4=1)
        sleep(delay)
        set_move(pos=pos, w1=0, w2=1, w3=0, w4=1)
        sleep(delay)
        set_move(pos=pos, w1=0, w2=1, w3=1, w4=0)
        sleep(delay)
        set_move(pos=pos, w1=1, w2=0, w3=1, w4=0)
        sleep(delay)
    set_move(pos=pos, w1=0, w2=0, w3=0, w4=0)
    print('Done.')

# 设定前向运动函数
def move_backward(pos, delay, steps):
    for i in range(steps):
        set_move(pos=pos, w1=1, w2=0, w3=1, w4=0)
        sleep(delay)
        set_move(pos=pos, w1=0, w2=1, w3=1, w4=0)
        sleep(delay)
        set_move(pos=pos, w1=0, w2=1, w3=0, w4=1)
        sleep(delay)
        set_move(pos=pos, w1=1, w2=0, w3=0, w4=1)
        sleep(delay)
    set_move(pos=pos, w1=0, w2=0, w3=0, w4=0)
    print('Done.')


# 上部电机运动函数
def up_fast(angle):
    step = abs(int((20000*angle)/360))
    if angle == 0:
        print('Done.')
    elif angle > 0:
        move_forward(pos='u', delay=0.0005, steps=step)
    else:
        move_backward(pos='u', delay=0.0005, steps=step)


def up_slow(angle):
    step = abs(int((20000*angle)/360))
    if angle == 0:
        print('Done.')
    elif angle > 0:
        move_forward(pos='u', delay=0.001, steps=step)
    else:
        move_backward(pos='u', delay=0.001, steps=step)



# 中部电机运动函数
def mid_fast(angle):
    step = abs(int((20000*angle)/360))
    if angle == 0:
        print('Done.')
    elif angle > 0:
        move_forward(pos='m', delay=0.0005, steps=step)
    else:
        move_backward(pos='m', delay=0.0005, steps=step)


def mid_slow(angle):
    step = abs(int((20000*angle)/360))
    if angle == 0:
        print('Done.')
    elif angle > 0:
        move_forward(pos='m', delay=0.001, steps=step)
    else:
        move_backward(pos='m', delay=0.001, steps=step)




# 下部电机运动函数
def down_fast(angle):
    step = abs(int((20000*angle)/360))
    if angle == 0:
        print('Done.')
    elif angle > 0:
        move_forward(pos='d', delay=0.0005, steps=step)
    else:
        move_backward(pos='d', delay=0.0005, steps=step)


def down_slow(angle):
    step = abs(int((20000*angle)/360))
    if angle == 0:
        print('Done.')
    elif angle > 0:
        move_forward(pos='d', delay=0.001, steps=step)
    else:
        move_backward(pos='d', delay=0.001, steps=step)
